#include "data_provider.h"
namespace vehicle {
namespace map {
bool DataProviderConfig::Update(ConfigInfo &config_info )
{
    std::lock_guard<std::mutex> lock(mutex_);
    config_info_ = config_info;
    return  true;
}

ConfigInfo const DataProviderConfig::config_info()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return  config_info_;
}

bool DataProviderEgoPath::Update(std::vector<STRoadPoint> &ego_path)
{
    std::lock_guard<std::mutex> lock(mutex_);
    ego_path_ = ego_path;
    return  true;
}

std::vector<STRoadPoint> const DataProviderEgoPath::ego_path()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return  ego_path_;
}

//bool DataProviderFencePoints::Update(std::vector<STRoadPoint> &fence_points_info)
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    for(int i=0;i<fence_points_info.size();i++)
//    {
//        double xy[2],lat_lon[2];
//        xy[0] = fence_points_info.at(i).global_x;
//        xy[1] = fence_points_info.at(i).global_y;
//        GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(xy,lat_lon);
//        fence_points_info.at(i).lat = lat_lon[0];
//        fence_points_info.at(i).lon = lat_lon[1];
//    }
//    fence_points_info_ = fence_points_info;
//    return  true;
//}

//std::vector<STRoadPoint> const DataProviderFencePoints::fence_points_info()
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    return  fence_points_info_;
//}

//bool DataProviderFencePoints::reset_fence_points_info()
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    if(fence_points_info_.size()>0)
//        fence_points_info_.clear();
//    return true;
//}

//bool DataProviderRoadPoints::Update(std::vector<STRoadPoint> &road_points)
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    road_points_ = road_points;
//    return true;

//}

//std::vector<STRoadPoint> const DataProviderRoadPoints::road_points()
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    return road_points_;
//}

//bool DataProviderGPSPoints::Update(STRoadPoints &gps_points)
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    gps_points_ = gps_points;
//    return true;
//}

//STRoadPoints const DataProviderGPSPoints::gps_points()
//{
//    std::lock_guard<std::mutex> lock(mutex_);
//    return gps_points_;
//}

bool DataProviderMapInfoPoints::Update(std::map<int, SetofMapInfo> &map_info_points)
{
    std::lock_guard<std::mutex> lock(mutex_);
    map_info_points_ = map_info_points;
    return true;
}

std::map<int, SetofMapInfo> const DataProviderMapInfoPoints::map_info_points()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return map_info_points_;
}

bool DataProviderSetofMapInfo::Update(SetofMapInfo &set_of_map_info)
{
    std::lock_guard<std::mutex> lock(mutex_);
    set_of_map_info_ = set_of_map_info;
    return true;
}

SetofMapInfo const DataProviderSetofMapInfo::set_of_map_info()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return set_of_map_info_;
}

bool DataProviderEgoPosition::Update(STRoadPoint &ego_position )
{
    std::lock_guard<std::mutex> lock(mutex_);
    ego_position_ = ego_position;
    return true;
}

STRoadPoint const DataProviderEgoPosition::ego_position()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return ego_position_;
}

bool DataProviderGPSIMUInfo::Update(lcmtypes::gps_imu_info_t &gps_imu_info)
{
    std::lock_guard<std::mutex> lock(mutex_);
    gps_imu_info_ = gps_imu_info;
    return  true;
}

lcmtypes::gps_imu_info_t  DataProviderGPSIMUInfo::GpsImuInfo()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return gps_imu_info_;
}


bool DataProviderVehicleStatusInfo::Update(lcmtypes::vehicle_status_t &vehicle_status)
{
    lcmtypes::gps_imu_info_t gps_imu_info_l = DataProviderGPSIMUInfo::Instance()->GpsImuInfo();
    double new_ll[2] = {gps_imu_info_l.latitude, gps_imu_info_l.longitude};
    double new_xy[2];

    STRoadPoint gps_point_l;
    GPSCoordinateTool::Instance()->gps_linearize_to_xy(new_ll,new_xy);
    gps_point_l.global_x = new_xy[0];
    gps_point_l.global_y = new_xy[1];
    gps_point_l.lon = new_ll[1];
    gps_point_l.lat = new_ll[0];

    gps_point_l.theta = vehicle::tools::cast_from_0_to_2PI_Angle(M_PI/2 - gps_imu_info_l.yaw);
    gps_point_l.yaw = vehicle::tools::cast_from_0_to_2PI_Angle(gps_imu_info_l.yaw);
    gps_point_l.location_state = gps_imu_info_l.locationStatus;
    DataProviderEgoPosition::Instance()->Update(gps_point_l);
    std::lock_guard<std::mutex> lock(mutex_);
    vehicle_status_ = vehicle_status;
    return  true;
}

lcmtypes::vehicle_status_t const DataProviderVehicleStatusInfo::VehicleStatus()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return vehicle_status_;
}

bool DataProviderMapInfo::Update(lcmtypes::map_info_t &map_info)
{
    std::lock_guard<std::mutex> lock(mutex_);
    map_info_ = map_info;
    return true;
}

lcmtypes::map_info_t const DataProviderMapInfo::MapInfo()
{
    std::lock_guard<std::mutex> lock(mutex_);
    return  map_info_;
}

}
}
